#include "force_control.h"
#include "zuc/nml_intf/zucpos2.h"
#include "axisgroup_robot.h"
#include "dyna/jaka_dyn_intf.h"
#include "log/zuclog.h"
#include "fct_para_common.h"
#include <sys/shm.h>
#include <sys/sem.h>
#include <math.h>

#define FCT_INFO(fmt, ...) zuclog_info("FCT", fmt, ##__VA_ARGS__)
#define FCT_ERR(fmt, ...) zuclog_error("FCT", fmt, ##__VA_ARGS__)
#define FCT_DEBUG(fmt, ...) zuclog_debug("FCT", fmt, ##__VA_ARGS__)

using namespace axisgroup;
using namespace ForceControl;

typedef union semun
{
    int val;
    struct semid_ds* buf;
    unsigned short* array;
} semun;

static int sem_get(key_t key);
static int sem_p(int sem_id);
static int sem_v(int sem_id);

axisgroup::ForceControlIntf::ForceControlIntf(Robot* grp) : grp_(grp) {}

void axisgroup::ForceControlIntf::enable_force_control(const ZucPose& cur_pose)
{
    if (compliant_enable_state_)
    {
        return;
    }
    compliant_enable_state_ = true;
    // 使能力控时，需要同步当前位置
    double pos[6] = {};
    zucpose_to_double(cur_pose, pos, true);
    grp_->dyna_->admittanceSynpos(pos);
    grp_->dyna_->resetAdmittanceVel();
    FCT_INFO("Force Control Enable, curr pose %lf %lf %lf %lf %lf", cur_pose.tran.x, cur_pose.tran.y, cur_pose.tran.z, cur_pose.a, cur_pose.b, cur_pose.c);
}

void axisgroup::ForceControlIntf::disable_force_control()
{
    if (!compliant_enable_state_)
    {
        return;
    }
    commnand_to_close_ = true;
    // #TODO 真正的关闭需要等到速度为零？
    // grp_->dyna_->resetAdmittanceVel();
    FCT_INFO("Force Control Need Disable");
}

int axisgroup::ForceControlIntf::admittanceRevise(const ZucPose& desired_pose, AxisGroupPose& cur_robot_pose, ZucPose& revised_pose)
{
    if (!force_sensor_.is_opened_)
    {
        FCT_ERR("Sensor is not ready!\n");
        return -1;
    }

    // #TODO 暂停时减速
    //    // if (tpManager_tpIsPausing() && fabs(zucmotStatus->current_vel) < 0.01)
    // {
    //     revised_pose = cur_robot_pose.pose;
    //     return 0;
    // }

    ZucPose currCartPos = cur_robot_pose.pose;

    int nortrak[6] = {0};    // 轨迹跟踪
    int enableOpt[7] = {0};  // 1-6，方向使能，7：0 TCP坐标系，1 世界坐标系
    double FT_user[6] = {};
    double FK[6] = {};
    double FC[6] = {0.0};  // DRAG 无需恒力
    for (int axis_num = 0; axis_num < 6; axis_num++)
    {
        nortrak[axis_num] = this->fc_config_.admit_fc_config[axis_num].enableNTrack;
        FC[axis_num] = this->fc_config_.admit_fc_config[axis_num].ftConstant;
        FT_user[axis_num] = this->fc_config_.admit_fc_config[axis_num].ftUser;
        FK[axis_num] = this->fc_config_.admit_fc_config[axis_num].ftReboundFK;
        enableOpt[axis_num] = this->fc_config_.admit_fc_config[axis_num].enableFt;
    }
    enableOpt[6] = fc_config_.admit_fc_frame;  // 7 去表示力控坐标系

    double cartPos[6] = {0.0};     // 当前位置
    double desiredPos[6] = {0.0};  // 规划位置
    double sensor_pos[6] = {0.0};  // 传感器TCP中心

    zucpose_to_double(currCartPos, cartPos, true);
    zucpose_to_double(desired_pose, desiredPos, true);

    double cartPos_new[6] = {0.0};
    double FT_act[6] = {0};  // 所选坐标的受力
    grp_->dyna_->setSensorCartPos(sensor_pos);
    grp_->dyna_->admittanceControl(cartPos, desiredPos, force_sensor_.raw_data_, enableOpt, nortrak, FC, FT_user, FK, FT_act, cartPos_new, 0);
    // grp_->dyna_->admittanceControlForCstForce(cartPos, desiredPos, force_sensor_.raw_data_, enableOpt, nortrak, FC, FT_user, FK, FT_act, cartPos_new, 0);   // 后续改用内迁S的函数

    for (int i = 0; i < 6; i++) { force_sensor_.actual_data_[i] = FT_act[i]; }
    for (int axis_num = 0; axis_num < 6; axis_num++)
    {
        if (fc_config_.torque_sensor_limit[axis_num] != 0 && fabs(FT_act[axis_num]) > fc_config_.torque_sensor_limit[axis_num])
        {
            return -1;
        }
    }

    double_to_zucpose(cartPos_new, revised_pose, true);
    return 0;
}

int axisgroup::ForceControlIntf::admittanceReviseForDrag(const ZucPose& desired_pose,
                                                         AxisGroupPose& cur_robot_pose,
                                                         ZucPose& revised_pose,
                                                         const double* End_Force_comb)
{
    if (!force_sensor_.is_opened_)
    {
        FCT_ERR("Sensor is not ready!\n");
        return -1;
    }

    ZucPose currCartPos = cur_robot_pose.pose;

    int nortrak[6] = {0};    // 轨迹跟踪
    int enableOpt[7] = {0};  // 1-6，方向使能，7：0 TCP坐标系，1 世界坐标系
    double FT_user[6] = {};
    double FK[6] = {};
    double FC[6] = {0.0};  // DRAG 无需恒力
    for (int axis_num = 0; axis_num < 6; axis_num++)
    {
        nortrak[axis_num] = this->fc_config_.admit_drag_config[axis_num].enableNTrack;
        FC[axis_num] = this->fc_config_.admit_drag_config[axis_num].ftConstant;
        FT_user[axis_num] = this->fc_config_.admit_drag_config[axis_num].ftUser;
        FK[axis_num] = this->fc_config_.admit_drag_config[axis_num].ftReboundFK;
        enableOpt[axis_num] = this->fc_config_.admit_drag_config[axis_num].enableFt;
    }
    enableOpt[6] = fc_config_.admit_fc_frame;  // 7 去表示力控坐标系

    double cartPos[6] = {0.0};
    zucpose_to_double(currCartPos, cartPos, true);
    double desiredPos[6] = {0.0};
    zucpose_to_double(desired_pose, desiredPos, true);

    double FT_act[6] = {0.0};
    double cartPos_new[6] = {0.0};
    grp_->dyna_->admittanceControlForDrag(
        cartPos, cur_robot_pose.jpos.val, desiredPos, force_sensor_.raw_data_, End_Force_comb, enableOpt, nortrak, FC, FT_user, FK, FT_act, cartPos_new, 1);

    double_to_zucpose(cartPos_new, revised_pose, true);
    return 0;
}

void axisgroup::ForceControlIntf::double_to_zucpose(const double* data, ZucPose& pose, bool to_deg)
{
    pose.tran.x = data[0];
    pose.tran.y = data[1];
    pose.tran.z = data[2];
    pose.a = data[3];
    pose.b = data[4];
    pose.c = data[5];
    if (to_deg)
    {
        pose.a *= 180.0 / PM_PI;
        pose.b *= 180.0 / PM_PI;
        pose.c *= 180.0 / PM_PI;
    }
}

void axisgroup::ForceControlIntf::zucpose_to_double(const ZucPose& pose, double* data, bool to_rad)
{
    data[0] = pose.tran.x;
    data[1] = pose.tran.y;
    data[2] = pose.tran.z;
    data[3] = pose.a;
    data[4] = pose.b;
    data[5] = pose.c;
    if (to_rad)
    {
        data[3] *= PM_PI / 180.0;
        data[4] *= PM_PI / 180.0;
        data[5] *= PM_PI / 180.0;
    }
}

int axisgroup::ForceControlIntf::admittanceReviseCombinedDrag(const ZucPose& desired_pose, AxisGroupPose& cur_robot_pose, ZucPose& revised_pose)
{
    // // 2. 参数复制
    // int i = 0;
    // double jPos[6] = {0.0};        // 当前位置
    // double sensor_pos[6] = {0.0};  // 传感器TCP中心

    // // 计算传感器中心位姿	@FIXME
    // zucmot_joint_t* joint = NULL;
    // double joint_fb[6] = {0.0};
    // // double joint_cmd[6] = {0.0};
    // for (i = 0; i < 6; i++)
    // {
    //     joint = &joints[i];
    //     joint_fb[i] = joint->pos_fb;
    // }

    // for (int i = 0; i < 6; i++) { jPos[i] = joints[i].pos_cmd; }

    // // 4.0 调用力控接口
    double End_Force[6] = {0.0};
    // double jTorque[6] = {0.0};
    // double jTorque_ori[6] = {0.0};
    // double jSensitivity[6] = {0.0};
    // double jSensitivityCorrection[2] = {0.75, 0.5};
    // grp_->dyna_->getBodyFtObserveSensitivity(jSensitivityCorrection);
    // grp_->dyna_->getJointExtTorque(jTorque);
    // grp_->dyna_->getJointSensitivityDrag(jSensitivity);

    // for (int i = 0; i < 6; i++) jTorque_ori[i] = jTorque[i];
    // for (int i = 0; i < 6; i++) { jSensitivity[i] *= jSensitivityCorrection[0]; }
    // for (int i = 0; i < 6; i++)
    // {
    //     if (fabs(jTorque[i]) > jSensitivity[i])
    //     {
    //         if (jTorque[i] > 0)
    //             jTorque[i] -= jSensitivity[i] * jSensitivityCorrection[1];
    //         else
    //             jTorque[i] += jSensitivity[i] * jSensitivityCorrection[1];
    //     }
    //     else
    //         jTorque[i] = 0.0;
    // }

    // grp_->dyna_->getSimulatedEndForce(jPos, End_Force, jTorque, jTorque_ori);
    admittanceReviseForDrag(desired_pose, cur_robot_pose, revised_pose, End_Force);
    return 0;
}

axisgroup::ForceControl::ForceSensor::ForceSensor()
{
    if (!sensor_shm_)
    {
        int shm_id = shmget((key_t)ForceSensor::SENSOR_SEM_KEY, sizeof(SensorShm), IPC_CREAT | 0666);
        if (shm_id < 0)
        {
            FCT_ERR("access servoj shared memory error , %d\n", shm_id);
        }
        sensor_shm_ = (SensorShm*)shmat(shm_id, NULL, 0);
        if (sensor_shm_ == NULL)
        {
            FCT_ERR("access servoj shared memory error 2\n");
        }
    }
}

void axisgroup::ForceControl::ForceSensor::update()
{
    double data[6] = {0.0};
    int sem_id = sem_get(ForceSensor::ForceSensor::SENSOR_SEM_KEY);
    if (sem_p(sem_id) == 0)
    {
        for (int i = 0; i < 6; i++) { data[i] = sensor_shm_->sensorStatus.torque[i]; }
        sem_v(sem_id);
    }

    if (fabs(data[0]) > 800 || fabs(data[1]) > 800 || fabs(data[2]) > 800 || fabs(data[3]) > 500 || fabs(data[4]) > 500 || fabs(data[5]) > 500)
    {
        if (error_data_count_++ > 10)
        {
            FCT_ERR("Sensor data is wrong! %lf, %lf, %lf, %lf, %lf, %lf\n", data[0], data[1], data[2], data[3], data[4], data[5]);
        }
    }
    else
    {
        for (int i = 0; i < 6; i++) { raw_data_[i] = data[i]; }
        error_data_count_ = 0;
    }
}

int sem_get(key_t key)
{
    int sem_id = semget(key, 1, 0666 | IPC_CREAT);
    union semun sem_union;
    sem_union.val = 1;
    if (semctl(sem_id, 0, SETVAL, sem_union) == -1)
    {
        return -1;
    }
    return sem_id;
}

//obtain a semaphore
int sem_p(int sem_id)
{
    struct sembuf sem_buf;
    sem_buf.sem_op = -1;
    sem_buf.sem_flg = SEM_UNDO;
    sem_buf.sem_num = 0;
    if (semop(sem_id, &sem_buf, 1) == -1)
    {
        return -1;
    }
    return 0;
}

//release an semaphore
int sem_v(int sem_id)
{
    struct sembuf sem_buf;
    sem_buf.sem_flg = SEM_UNDO;
    sem_buf.sem_num = 0;
    sem_buf.sem_op = 1;
    if (semop(sem_id, &sem_buf, 1) == -1)
    {
        return -1;
    }
    return 0;
}